home *** CD-ROM | disk | FTP | other *** search
/ Our Solar System / Our Solar System.iso / shuttle / seesat3 / sgp4.c < prev    next >
Encoding:
C/C++ Source or Header  |  1990-12-24  |  8.4 KB  |  294 lines
  1. /*
  2. SGP4.C
  3. by Paul S. Hirose, 1990 June 19
  4. Orbit prediction model for the SEESAT satellite tracking program.
  5.  
  6. 300 - 399
  7. */
  8.  
  9. /* library functions used in this file:
  10.     atan2 cos fabs sin sqrt
  11. */
  12.  
  13. #include "b:seesat.h"    /* the global header */
  14.  
  15. extern double atan2(), cos(), fabs(), sin(), sqrt();
  16.  
  17. /* If true, BSTAR will be estimated if a value is not provided in the element
  18. set.  Estimation formula courtesy of Ted Molczan. */
  19. #define ESTB 1
  20.  
  21. /* If true, velocities will be loaded in global struct "satdot".  Otherwise,
  22. we can save a few lines of code. */
  23. #define VEL 0
  24.  
  25. static double
  26. a, ao, aodp, axn, aycof, ayn, aynl, a1, a3ovk2, beta, betal, betao,
  27. betao2, capu, coef, coef1, cosepw, cosik, cosio, cosnok, cosu, cosuk,
  28. cos2u, c1, c1sq, c2, c3, c4, c5, delm, delmo, delo, delomg, del1, d2,
  29. d3, d4, e, ecose, eeta, elsq, eosq, epw, esine, eta, etasq, omega,
  30. omgcof, omgadf, omgdot, perige, pinvsq, pl, psisq, qoms24, r, rdot,
  31. rdotk, rfdot, rfdotk, rk, sinepw, sinik, sinio, sinmo, sinnok, sinu,
  32. sinuk, sin2u, s4, tcube, temp, tfour, tempa, tempe, templ, temp1,
  33. temp2, temp3, temp4, temp5, temp6, theta2, theta4, tsi, tsince, tsq,
  34. t2cof, t3cof, t4cof, t5cof, u, uk, ux, uy, uz, vx, vy, vz, xhdot1,
  35. xinck, xl, xlcof, xll, xlt, xmcof, xmdf, xmdot, xmp, xmx, xmy, xn,
  36. xnodcf, xnoddf, xnode, xnodek, xnodot, xnodp, x1cof, x1mth2, x1m5th,
  37. x3thm1, x7thm1;
  38.  
  39. static int i, isimp;
  40.  
  41. void
  42. sgp4(tsince)
  43. double tsince;
  44. {
  45.     DTEST((300, 1, &iflag));
  46.     FTEST((301, 1, 3, &tsince));
  47.  
  48.     if (iflag) {
  49.         /* this is first call of sgp4() in prediction run */
  50.  
  51.         /* Recover original mean motion (xnodp) and semimajor axis
  52.         (aodp) from input elements. */
  53.  
  54.         a1 = POW(xke / xno, tothrd);
  55.         cosio = cos(xincl);
  56.         theta2 = cosio * cosio;
  57.         x3thm1 = 3. * theta2 - 1.;
  58.         eosq = eo * eo;
  59.         betao2 = 1. - eosq;
  60.         betao = sqrt(betao2);
  61.         del1 = 1.5 * ck2 * x3thm1 / (a1 * a1 * betao * betao2);
  62.         ao = a1 * (1. - del1 * (.5 * tothrd + del1 * (1. + 134.
  63.          / 81. * del1)));
  64.         delo = 1.5 * ck2 * x3thm1 / (ao * ao * betao * betao2);
  65.         xnodp = xno / (1. + delo);
  66.         aodp = ao / (1. - delo);
  67.  
  68.         /* Initialization.  For perigee less than 220 km, the
  69.         "isimp" flag is set and the equations are truncated to linear
  70.         variation in sqrt(a) and quadratic variation in mean anomaly.
  71.         Also, the c3, delta omega, and delta m terms are dropped. */
  72.  
  73.         if (aodp * (1. - eo) <  220. / xkmper + 1.)
  74.             isimp = 1;
  75.         else
  76.             isimp = 0;
  77.  
  78.         s4 = s;
  79.         qoms24 = qoms2t;
  80.         perige = (aodp * (1. - eo) - 1.) * xkmper;
  81.  
  82.         /* For perigee below 156 km, the values of
  83.         s and qoms2t are altered. */
  84.         if (perige < 156.) {
  85.             if (perige > 98.)
  86.                 s4 = perige - 78.;
  87.             else
  88.                 s4 = 20.;
  89.             qoms24 = POW((120. - s4) / xkmper, 4.);
  90.             s4 = s4 / xkmper + 1.;
  91.         }
  92.         pinvsq = 1. / (aodp * aodp * betao2 * betao2);
  93.         tsi = 1. / (aodp - s4);
  94.         eta = aodp * eo * tsi;
  95.         etasq = eta * eta;
  96.         eeta = eo * eta;
  97.         psisq = fabs(1. - etasq);
  98.         coef = qoms24 * POW(tsi, 4.);
  99.         coef1 = coef / POW(psisq, 3.5);
  100.         c2 = coef1 * xnodp * (aodp * (1. + 1.5 * etasq + eeta *
  101.          (4. + etasq)) + .75 * ck2 * tsi / psisq * x3thm1 *
  102.          (8. + 3. * etasq * (8. + etasq)));
  103. #if ESTB
  104.         if (bstar == 0.)    /* field in element set was blank */
  105.             bstar = tothrd * xndt2o / (xno * c2);
  106.         ETEST((305, 1, 2, &bstar));
  107. #endif
  108.         c1 = bstar * c2;
  109.         sinio = sin(xincl);
  110.         a3ovk2 = -xj3 / ck2;
  111.         c3 = coef * tsi * a3ovk2 * xnodp * sinio / eo;
  112.         x1mth2 = 1. - theta2;
  113.         c4 = 2. * xnodp * coef1 * aodp * betao2 * (eta
  114.          * (2. + .5 * etasq) + eo * (.5 + 2. * etasq) - 2. * ck2
  115.          * tsi / (aodp * psisq) * (-3. * x3thm1 * (1. - 2. * eeta
  116.          + etasq * (1.5 -.5 * eeta)) + .75 * x1mth2 * (2. * etasq
  117.          - eeta * (1. + etasq)) * cos(2. * omegao)));
  118.         c5 = 2. * coef1 * aodp * betao2 * (1. + 2.75 * (etasq + eeta)
  119.          + eeta * etasq);
  120.         theta4 = theta2 * theta2;
  121.         temp1 = 3. * ck2 * pinvsq * xnodp;
  122.         temp2 = temp1 * ck2 * pinvsq;
  123.         temp3 = 1.25 * ck4 * pinvsq * pinvsq * xnodp;
  124.         xmdot = xnodp + .5 * temp1 * betao * x3thm1 + .0625 * temp2
  125.          * betao * (13. - 78. * theta2 + 137. * theta4);
  126.         x1m5th = 1. - 5. * theta2;
  127.         omgdot = -.5 * temp1 * x1m5th + .0625 * temp2 * (7. - 114.
  128.          * theta2 + 395. * theta4) + temp3 * (3. - 36. * theta2
  129.          + 49. * theta4);
  130.         xhdot1 = -temp1 * cosio;
  131.         xnodot = xhdot1 + (.5 * temp2 * (4. - 19. * theta2) + 2.
  132.          * temp3 * (3. - 7. * theta2)) * cosio;
  133.         omgcof = bstar * c3 * cos(omegao);
  134.         xmcof = -tothrd * coef * bstar / eeta;
  135.         xnodcf = 3.5 * betao2 * xhdot1 * c1;
  136.         t2cof = 1.5 * c1;
  137.         xlcof = .125 * a3ovk2 * sinio * (3. + 5. * cosio)
  138.          / (1. + cosio);
  139.         aycof = .25 * a3ovk2 * sinio;
  140.         delmo = POW(1. + eta * cos(xmo), 3.);
  141.         sinmo = sin(xmo);
  142.         x7thm1 = 7. * theta2 - 1.;
  143.         if (!isimp) {
  144.             c1sq = c1 * c1;
  145.             d2 = 4. * aodp * tsi * c1sq;
  146.             temp = d2 * tsi * c1 / 3.;
  147.             d3 = (17. * aodp + s4) * temp;
  148.             d4 = .5 * temp * aodp * tsi
  149.              * (221. * aodp + 31. * s4) * c1;
  150.             t3cof = d2 + 2. * c1sq;
  151.             t4cof = .25 * (3. * d3 + c1 * (12. * d2 + 10.
  152.              * c1sq));
  153.             t5cof = .2 * (3. * d4 + 12. * c1 * d3 + 6. * d2 * d2
  154.              + 15. * c1sq * (2. * d2 + c1sq));
  155.         } iflag = 0;
  156.     }
  157.  
  158.     /* update for secular gravity and atmospheric drag */
  159.  
  160.     xmdf = xmo + xmdot * tsince;
  161.     omgadf = omegao + omgdot * tsince;
  162.     xnoddf = xnodeo + xnodot * tsince;
  163.     omega = omgadf;
  164.     xmp = xmdf;
  165.     tsq = tsince * tsince;
  166.     xnode = xnoddf + xnodcf * tsq;
  167.     tempa = 1. - c1 * tsince;
  168.     tempe = bstar * c4 * tsince;
  169.     templ = t2cof * tsq;
  170.     if (!isimp) {
  171.         delomg = omgcof * tsince;
  172.         delm = xmcof * (POW(1. + eta * cos(xmdf), 3.) - delmo);
  173.         temp = delomg + delm;
  174.         xmp = xmdf + temp;
  175.         omega = omgadf - temp;
  176.         tcube = tsq * tsince;
  177.         tfour = tsince * tcube;
  178.         tempa = tempa - d2 * tsq - d3 * tcube - d4 * tfour;
  179.         tempe = tempe + bstar * c5 * (sin(xmp) - sinmo);
  180.         templ = templ + t3cof * tcube + tfour * (t4cof + tsince
  181.          * t5cof);
  182.     } a = aodp * tempa * tempa;
  183.     e = eo - tempe;
  184.     xl = xmp + omega + xnode + xnodp * templ;
  185.     beta = sqrt(1. - e * e);
  186.     xn = xke / POW(a, 1.5);
  187.  
  188.     /* long period periodics */
  189.  
  190.     axn = e * cos(omega);
  191.     temp = 1. / (a * beta * beta);
  192.     xll = temp * xlcof * axn;
  193.     aynl = temp * aycof;
  194.     xlt = xl + xll;
  195.     ayn = e * sin(omega) + aynl;
  196.  
  197.     /* solve Kepler's equation */
  198.  
  199.     capu = fmod2p(xlt - xnode);
  200.  
  201.     FTEST((302, 3, 3, &xlt, &xnode, &capu));
  202.  
  203.     temp2 = capu;
  204.     for (i = -10; i; ++i) {
  205.         sinepw = sin(temp2);
  206.         cosepw = cos(temp2);
  207.         temp3 = axn * sinepw;
  208.         temp4 = ayn * cosepw;
  209.         temp5 = axn * cosepw;
  210.         temp6 = ayn * sinepw;
  211.         epw = (capu - temp4 + temp3 - temp2) / (1. - temp5 - temp6)
  212.          + temp2;
  213.         if (fabs(epw - temp2) <= e6a)
  214.             break;
  215.         temp2 = epw;
  216.     }
  217.     DTEST((306, 1, &i));
  218.  
  219.     /* short period preliminary quantities */
  220.  
  221.     ecose = temp5 + temp6;
  222.     esine = temp3 - temp4;
  223.     elsq = axn * axn + ayn * ayn;
  224.     temp = 1. - elsq;
  225.     pl = a * temp;
  226.     r = a * (1. - ecose);
  227.     temp1 = 1. / r;
  228. #if VEL
  229.     rdot = xke * sqrt(a) * esine * temp1;
  230.     rfdot = xke * sqrt(pl) * temp1;
  231. #endif
  232.     temp2 = a * temp1;
  233.     betal = sqrt(temp);
  234.     temp3 = 1. / (1. + betal);
  235.     cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
  236.     sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
  237.     u = atan2(sinu, cosu);
  238.     sin2u = 2. * sinu * cosu;
  239.     cos2u = 2. * cosu * cosu - 1.;
  240.     temp = 1. / pl;
  241.     temp1 = ck2 * temp;
  242.     temp2 = temp1 * temp;
  243.  
  244.     /* update for short periodics */
  245.  
  246.     rk = r * (1. - 1.5 * temp2 * betal * x3thm1) + .5 * temp1 * x1mth2
  247.      * cos2u;
  248.     uk = u - .25 * temp2 * x7thm1 * sin2u;
  249.     xnodek = xnode + 1.5 * temp2 * cosio * sin2u;
  250.     xinck = xincl + 1.5 * temp2 * cosio * sinio * cos2u;
  251. #if VEL
  252.     rdotk = rdot - xn * temp1 * x1mth2 * sin2u;
  253.     rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1);
  254. #endif
  255.  
  256.     /* orientation vectors */
  257.  
  258.     sinuk = sin(uk);
  259.     cosuk = cos(uk);
  260.     sinik = sin(xinck);
  261.     cosik = cos(xinck);
  262.     sinnok = sin(xnodek);
  263.     cosnok = cos(xnodek);
  264.     xmx = -sinnok * cosik;
  265.     xmy = cosnok * cosik;
  266.     ux = xmx * sinuk + cosnok * cosuk;
  267.     uy = xmy * sinuk + sinnok * cosuk;
  268.     uz = sinik * sinuk;
  269.  
  270. #if VEL
  271.     vx = xmx * cosuk - cosnok * sinuk;
  272.     vy = xmy * cosuk - sinnok * sinuk;
  273.     vz = sinik * cosuk;
  274. #endif
  275.  
  276.     /* position */
  277.  
  278.     sat.x = rk * ux;
  279.     sat.y = rk * uy;
  280.     sat.z = rk * uz;
  281.  
  282. #if VEL
  283.     satdot.x = rdotk * ux + rfdotk * vx;
  284.     satdot.y = rdotk * uy + rfdotk * vy;
  285.     satdot.z = rdotk * uz + rfdotk * vz;
  286. #endif
  287.  
  288.     FTEST((303, 3, 6, &sat.x, &sat.y, &sat.z));
  289.     FTEST((304, 3, 6, &satdot.x, &satdot.y, &satdot.z));
  290. }
  291.  cos2u;
  292.     uk = u - .25 * temp2 * x7thm1 * sin2u;
  293.     xnodek = xnode + 1.5 * temp2 * cosio * sin2u;
  294.     xinc